//
// Created by xiang on 2022/3/23.
//

#include "ch6/submap.h"
#include <glog/logging.h>

namespace sad {

// 把另一个submap中的占据栅格复制到本地图中
void Submap::SetOccuFromOtherSubmap(std::shared_ptr<Submap> other) {
    // 获取子图中的关键帧
    auto frames_in_other = other->GetFrames();
    // 取最近10个帧
    for (size_t i = frames_in_other.size() - 10; i < frames_in_other.size(); ++i) {
        if (i > 0) {
            occu_map_.AddLidarFrame(frames_in_other[i]);
        }
    }
    // 
    field_.SetFieldImageFromOccuMap(occu_map_.GetOccupancyGrid());
}

// 将frame与本submap进行匹配，计算frame->pose
bool Submap::MatchScan(std::shared_ptr<Frame> frame) {
    LOG(INFO) << "[MatchScan] pose:" << PrintData(pose_);
    LOG(INFO) << "[MatchScan] pose_submap:" << PrintData(frame->pose_submap_);
    field_.SetSourceScan(frame->scan_);
    // field_.AlignG2O(frame->pose_submap_);
    field_.AlignGaussNewton(frame->pose_submap_);
    // frame->pose_  T_w_c 每帧扫描数据的世界坐标
    // pose_ T_w_s 子地图的世界坐标
    // frame->pose_submap_ 每帧扫描数据的子地图坐标
    frame->pose_ = pose_ * frame->pose_submap_;  // T_w_c = T_w_s * T_s_c  见P223
    LOG(INFO) << "[MatchScan]frame pose:" << PrintData(frame->pose_);
    return true;
}

// 在栅格地图中增加一个帧
void Submap::AddScanInOccupancyMap(std::shared_ptr<Frame> frame) {
    // occu_map_.AddLidarFrame(frame, OccupancyMap::GridMethod::MODEL_POINTS);  // 更新栅格地图中的格子
    occu_map_.AddLidarFrame(frame, OccupancyMap::GridMethod::BRESENHAM);
    
    field_.SetFieldImageFromOccuMap(occu_map_.GetOccupancyGrid());           // 更新场函数图像
}

// 判定当前的scan是否有位于submap外部的点
bool Submap::HasOutsidePoints() const { return occu_map_.HasOutsidePoints(); }

void Submap::SetPose(const SE2& pose) {
    pose_ = pose;
    occu_map_.SetPose(pose);
    field_.SetPose(pose);
}

// 当子地图的位姿更新时，重设每个frame的世界位姿
void Submap::UpdateFramePoseWorld() {
    for (auto& frame : frames_) {
        frame->pose_ = pose_ * frame->pose_submap_;
    }
}

}  // namespace sad